/**
 * @file ebex_genetic_chromosome.c
 *
 * @date Oct 24, 2011
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2011 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <stdbool.h>
#include <stdint.h>

#include <ebex_quaternion.h>
#include <ebex_genetic_chromosome.h>

static const uint8_t empty_chromo[] = "\0";

static void ebex_genetic_chromosome_realign_alleles(organism_t *m_entity)
{
	size_t i;
	size_t offset = 0;

	for (i = 0; i < m_entity->allocated_chromosomes; i++)
	{
		switch(m_entity->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				m_entity->chromosome[i].dbl_allele = (double *) ((char*)m_entity->allele_data + offset);
				offset += (m_entity->chromosome[i].length * sizeof(double));
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				m_entity->chromosome[i].q_allele = (ebex_quat_t*) ((char*)m_entity->allele_data + offset);
				offset += (m_entity->chromosome[i].length * sizeof(ebex_quat_t));
				break;
			default:
				die("Unknown chromosome type");
				break;
		}
	}
}

bool ebex_genetic_chromosome_double_allocate(population_t *m_pop, organism_t *embryo, size_t m_numchromosomes, size_t m_length)
{
	size_t i;
	size_t newsize;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!embryo)
		die("Null pointer to entity structure passed.");
	if (!m_numchromosomes || !m_length)
		die("Bad length argument!");

	newsize = m_numchromosomes * m_length * sizeof(double);

	if (!embryo->chromosome)
	{
		embryo->chromosome = e_calloc(m_pop->num_chromosomes, sizeof(chromosome_t));
		embryo->allele_data = e_memalign(newsize);
	}
	else
	{
		embryo->allele_data = e_rememalign(embryo->allele_data, embryo->allele_size + newsize);
	}

	if (!embryo->chromosome) die("Unable to allocate memory");

	e_memset((char*)embryo->allele_data + embryo->allele_size, 0, newsize);

	for (i = 0; i < m_numchromosomes; i++)
	{
		embryo->chromosome[i + embryo->allocated_chromosomes].length = m_length;
		embryo->chromosome[i + embryo->allocated_chromosomes].type = CHROMOSOME_TYPE_DOUBLE;
	}

	embryo->allele_size += newsize;
	embryo->allocated_chromosomes += m_numchromosomes;

	ebex_genetic_chromosome_realign_alleles(embryo);

	return true;
}

void ebex_genetic_chromosome_free(population_t *m_pop, organism_t *corpse)
{

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!corpse)
		die("Null pointer to entity structure passed.");

	if (!corpse->chromosome)
    		die("This entity already contains no chromosomes.");

	e_free(corpse->allele_data);
	e_free(corpse->chromosome);
	corpse->chromosome = NULL;
	corpse->allele_data = NULL;

	return;
}

/**
 * Copy a chromosome from one entity to another
 * @param m_pop Population containing the entity
 * @param m_src Source entity
 * @param m_dst Destination entity
 * @param m_chromosome Chromosome index
 */
size_t ebex_genetic_chromosome_double_copy(const population_t *m_pop, organism_t *m_src, organism_t *m_dst, const size_t m_chromosome)
{
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_src || !m_dst)
		die("Null pointer to entity structure passed.");
	if (!m_src->chromosome || !m_dst->chromosome)
		die("Entity has no chromosomes.");
	if (m_src->chromosome->type != CHROMOSOME_TYPE_DOUBLE)
		die("Wrong chromosome type.");

	memcpy(m_dst->chromosome[m_chromosome].dbl_allele, m_src->chromosome[m_chromosome].dbl_allele, m_dst->chromosome[m_chromosome].length * sizeof(double));
	return m_dst->chromosome[m_chromosome].length * sizeof(double);
}

/**
 * Serializes the chromosome for writing.  Likely not portable as depends on machine representation of double
 * @param m_pop Population pointer
 * @param m_entity Entity Pointer
 * @param m_dst Pointer to destination character array (will be set to the chromosome)
 * @return Number of bytes available to be written
 */
size_t ebex_genetic_chromosome_to_bytes(const population_t *m_pop, organism_t *m_entity, const uint8_t **m_dst)
{
	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_entity)
		die("Null pointer to entity structure passed.");

	if (!m_entity->chromosome)
	{
		*m_dst = empty_chromo;
		return 0;
	}

	*m_dst = (uint8_t *) m_entity->allele_data;

	return m_entity->allele_size;
}

/**
 * Creates a set of double chromosomes from a byte string.
 * @param m_pop Population containing #m_entity
 * @param m_entity Entity to receive the chromosomes
 * @param m_bytestring Byte string containing effective set of chromosomes.  Must be at least chromosome_size
 */
void ebex_genetic_chromosome_from_bytes(const population_t *m_pop, organism_t *m_entity, uint8_t *m_bytestring)
{

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_entity)
		die("Null pointer to entity structure passed.");

	if (!m_entity->chromosome)
		die("Entity has no chromosomes.");

	memcpy(m_entity->allele_data, m_bytestring, m_entity->allele_size);

	return;
}

char *ebex_genetic_chromosome_double_to_string(const population_t *m_pop, const organism_t *m_entity, char *m_dest, size_t *m_destlen, const size_t m_chromosome)
{
	size_t j;
	int k = 0;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_entity)
		die("Null pointer to entity structure passed.");

	for (j = 0; j < m_entity->chromosome[m_chromosome].length; j++)
	{
		k += snprintf(NULL, 0, "%f ", m_entity->chromosome[m_chromosome].dbl_allele[j]);
	}

	if (!m_dest || (int) *m_destlen < k + 1)
	{
		*m_destlen = k + 1;
		m_dest = e_realloc(m_dest, sizeof(char) * *m_destlen);
	}

	if (!m_entity->chromosome)
	{
		m_dest[0] = ' ';
		m_dest[1] = '\0';
		return m_dest;
	}

	k = 0;
	for (j = 0; j < m_entity->chromosome[m_chromosome].length; j++)
	{
		k += snprintf(&(m_dest[k]), *m_destlen - k, "%f ", m_entity->chromosome[m_chromosome].dbl_allele[j]);
		if (k < 0)
		{
			die("Error outputting chromosome!");
		}
	}

	m_dest[k - 1] = '\0';

	return m_dest;
}

bool ebex_genetic_chromosome_quaternion_allocate(population_t *m_pop, organism_t *embryo, size_t m_numchromosomes, size_t m_length)
{
	size_t i;
	size_t newsize;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!embryo)
		die("Null pointer to entity structure passed.");
	if (!m_numchromosomes || !m_length)
		die("Bad length argument!");

	newsize = m_numchromosomes * m_length * sizeof(ebex_quat_t);

	if (!embryo->chromosome)
	{
		embryo->chromosome = e_calloc(m_pop->num_chromosomes, sizeof(chromosome_t));
		embryo->allele_data = e_memalign(newsize);
	}
	else
	{
		embryo->allele_data = e_rememalign(embryo->allele_data, embryo->allele_size + newsize);
	}

	if (!embryo->chromosome) die("Unable to allocate memory");

	e_memset((char*)embryo->allele_data + embryo->allele_size, 0, newsize);


	for (i = 0; i < m_numchromosomes; i++)
	{
		embryo->chromosome[i + embryo->allocated_chromosomes].length = m_length;
		embryo->chromosome[i + embryo->allocated_chromosomes].type = CHROMOSOME_TYPE_QUATERNION;
	}

	embryo->allele_size += newsize;
	embryo->allocated_chromosomes += m_numchromosomes;
	ebex_genetic_chromosome_realign_alleles(embryo);

	return true;
}

char *ebex_genetic_chromosome_quaternion_to_string(const population_t *m_pop, const organism_t *m_entity, char *m_dest, size_t *m_destlen, const size_t m_chromosome)
{
	size_t j;
	int k = 0;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_entity)
		die("Null pointer to entity structure passed.");

	if (!m_entity->chromosome)
	{
		m_dest[0] = ' ';
		m_dest[1] = '\0';
		return m_dest;
	}

	for (j = 0; j < m_entity->chromosome[m_chromosome].length; j++)
	{
		k += snprintf(NULL, 0, "(%f : %f : %f : %f) ",
				m_entity->chromosome[m_chromosome].q_allele[j].x, m_entity->chromosome[m_chromosome].q_allele[j].y,
				m_entity->chromosome[m_chromosome].q_allele[j].z, m_entity->chromosome[m_chromosome].q_allele[j].w);
	}


	if (!m_dest || (int) *m_destlen < k + 1)
	{
		*m_destlen = k + 1;
		m_dest = e_realloc(m_dest, sizeof(char) * *m_destlen);
	}

	k = 0;
	for (j = 0; j < m_entity->chromosome[m_chromosome].length; j++)
	{
		k += snprintf(&(m_dest[k]), *m_destlen - k, "(%f : %f : %f : %f) ",
				m_entity->chromosome[m_chromosome].q_allele[j].x, m_entity->chromosome[m_chromosome].q_allele[j].y,
				m_entity->chromosome[m_chromosome].q_allele[j].z, m_entity->chromosome[m_chromosome].q_allele[j].w);

		if (k < 0)
		{
			die("Error outputting chromosome!");
		}
	}

	m_dest[k - 1] = '\0';

	return m_dest;
}

size_t ebex_genetic_chromosome_quaternion_copy(const population_t *m_pop, organism_t *m_src, organism_t *m_dst, const size_t m_chromosome)
{
	ebex_quat_t *tmp_ptr = NULL;

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_src || !m_dst)
		die("Null pointer to entity structure passed.");
	if (!m_src->chromosome || !m_dst->chromosome)
		die("Entity has no chromosomes.");
	if (m_src->chromosome[m_chromosome].type != CHROMOSOME_TYPE_QUATERNION || m_dst->chromosome[m_chromosome].type != CHROMOSOME_TYPE_QUATERNION)
		die("Wrong chromosome type.");

	tmp_ptr = m_dst->chromosome[m_chromosome].q_allele;
	memcpy(&m_dst->chromosome[m_chromosome], &m_src->chromosome[m_chromosome], sizeof(chromosome_t));
	m_dst->chromosome[m_chromosome].q_allele = tmp_ptr;

	memcpy(m_dst->chromosome[m_chromosome].q_allele, m_src->chromosome[m_chromosome].q_allele, m_dst->chromosome[m_chromosome].length * sizeof(ebex_quat_t));
	return m_dst->chromosome[m_chromosome].length * sizeof(ebex_quat_t);
}

bool ebex_genetic_chromosome_verify(population_t *m_pop, organism_t *m_entity)
{

	if (!m_pop)
		die("Null pointer to population structure passed.");
	if (!m_entity)
		die("Null pointer to organism passed.");

	if (!m_entity->chromosome)
    		die("This entity already contains no chromosomes.");

	for (size_t i = 0; i < m_pop->num_chromosomes; i++)
	{
		switch(m_entity->chromosome[i].type)
		{
			case CHROMOSOME_TYPE_DOUBLE:
				for (size_t j = 0; j < m_entity->chromosome[i].length; j++)
				{
					m_entity->chromosome[i].dbl_allele[j] =
						CLAMP(m_entity->chromosome[i].dbl_allele[j],
								m_entity->chromosome[i].allele_min_double,
								m_entity->chromosome[i].allele_max_double);
				}
				break;
			case CHROMOSOME_TYPE_QUATERNION:
				for (size_t j = 0; j < m_entity->chromosome[i].length; j++)
				{
					ebex_quat_normalize(&m_entity->chromosome[i].q_allele[j],&m_entity->chromosome[i].q_allele[j]);
				}
				break;
			default:
				break;
		}
	}

	return true;
}
